function optimized_graph = optimize_pose_graph(pose_graph, max_iter)
% 位姿图优化（Gauss-Newton方法，修复版）
% 输入: pose_graph-位姿图, max_iter-最大迭代次数
% 输出: optimized_graph-优化后的位姿图结构体

    n_poses = size(pose_graph.poses, 2);
    n_landmarks = pose_graph.landmark_map.Count;
    
    % 初始化状态向量 [poses; landmarks]
    x = zeros(3 * n_poses + 2 * n_landmarks, 1);
    
    % 填充位姿
    for i = 1:n_poses
        x((i-1)*3+1:i*3) = pose_graph.poses(:, i);
    end
    
    % 填充路标（关键修复：需要从观测中估计初始位置）
    lm_keys = cell2mat(keys(pose_graph.landmark_map));
    lm_map = containers.Map('KeyType', 'int32', 'ValueType', 'int32');
    for i = 1:length(lm_keys)
        lm_id = lm_keys(i);
        lm_idx_in_map = pose_graph.landmark_map(lm_id);
        
        % 从观测边中估计路标初始位置
        lm_pos = estimate_landmark_position(pose_graph, lm_idx_in_map, n_poses);
        
        x(3*n_poses + (i-1)*2+1:3*n_poses + i*2) = lm_pos;
        lm_map(lm_id) = i;
    end
    
    % 固定第一个位姿
    fixed_pose = 1;
    
    fprintf('  图优化: %d个位姿, %d个路标, %d次迭代\n', n_poses, n_landmarks, max_iter);
    
    % Gauss-Newton迭代
    chi2_prev = inf;
    x_prev = x;  % 保存上一步状态
    
    for iter = 1:max_iter
        % 显示进度
        if mod(iter, max(1, round(max_iter/5))) == 0 || iter == 1 || iter == max_iter
            percent = round(100 * iter / max_iter);
            fprintf('    优化迭代 [%3d%%] %2d/%2d\r', percent, iter, max_iter);
            if iter == max_iter
                fprintf('\n');
            end
        end
        H = sparse(length(x), length(x));
        b = zeros(length(x), 1);
        chi2 = 0;
        
        % 遍历所有边
        for e = 1:length(pose_graph.edges)
            edge = pose_graph.edges(e);
            
            if strcmp(edge.type, 'odometry')
                % 里程计约束
                i = edge.from;
                j = edge.to;
                
                if i < 1 || j < 1 || i > n_poses || j > n_poses
                    continue;
                end
                
                xi = x((i-1)*3+1:i*3);
                xj = x((j-1)*3+1:j*3);
                
                % 测量值
                z = edge.measurement;
                v = z(1);
                w = z(2);
                dt = edge.dt;
                
                % 预测
                theta_i = xi(3);
                if abs(w) < 1e-6
                    pred_x = xi(1) + v * dt * cos(theta_i);
                    pred_y = xi(2) + v * dt * sin(theta_i);
                else
                    pred_x = xi(1) + (v/w) * (sin(theta_i + w*dt) - sin(theta_i));
                    pred_y = xi(2) + (v/w) * (-cos(theta_i + w*dt) + cos(theta_i));
                end
                
                % 误差（仅位置）
                error = [xj(1) - pred_x; xj(2) - pred_y];
                
                % 雅可比
                J_i = zeros(2, 3);
                J_j = [eye(2), zeros(2,1)];
                
                if abs(w) < 1e-6
                    J_i(1,1) = -1;
                    J_i(2,2) = -1;
                    J_i(1,3) = -v*dt*sin(theta_i);
                    J_i(2,3) = v*dt*cos(theta_i);
                else
                    J_i(1,1) = -1;
                    J_i(2,2) = -1;
                    J_i(1,3) = -(v/w)*(cos(theta_i+w*dt) - cos(theta_i));
                    J_i(2,3) = -(v/w)*(sin(theta_i+w*dt) - sin(theta_i));
                end
                
                % 信息矩阵（调整权重平衡里程计和观测）
                info = edge.info * 0.1;
                
                % 累加到H和b
                idx_i = (i-1)*3+(1:3);
                idx_j = (j-1)*3+(1:3);
                
                H(idx_i, idx_i) = H(idx_i, idx_i) + J_i' * info * J_i;
                H(idx_i, idx_j) = H(idx_i, idx_j) + J_i' * info * J_j;
                H(idx_j, idx_i) = H(idx_j, idx_i) + J_j' * info * J_i;
                H(idx_j, idx_j) = H(idx_j, idx_j) + J_j' * info * J_j;
                
                b(idx_i) = b(idx_i) + J_i' * info * error;
                b(idx_j) = b(idx_j) + J_j' * info * error;
                
                chi2 = chi2 + error' * info * error;
                
            elseif strcmp(edge.type, 'landmark')
                % 路标观测约束
                pose_id = edge.pose_id;
                lm_id = edge.landmark_id;
                
                if pose_id < 1 || pose_id > n_poses || ~lm_map.isKey(lm_id)
                    continue;
                end
                
                lm_idx = double(lm_map(lm_id));  % 转换为double避免类型错误
                
                xi = x((pose_id-1)*3+1:pose_id*3);
                lm = x(3*n_poses + (lm_idx-1)*2+1:3*n_poses + lm_idx*2);
                
                z = edge.measurement;
                
                % 预测观测
                dx = lm(1) - xi(1);
                dy = lm(2) - xi(2);
                q = dx^2 + dy^2;
                
                if q < 1e-6
                    continue;
                end
                
                range_pred = sqrt(q);
                bearing_pred = wrapToPi(atan2(dy, dx) - xi(3));
                z_pred = [range_pred; bearing_pred];
                
                % 创新
                error = z - z_pred;
                error(2) = wrapToPi(error(2));
                
                % 雅可比
                J_pose = zeros(2, 3);
                J_pose(1,1) = -dx / range_pred;
                J_pose(1,2) = -dy / range_pred;
                J_pose(1,3) = 0;
                J_pose(2,1) = dy / q;
                J_pose(2,2) = -dx / q;
                J_pose(2,3) = -1;
                
                J_lm = zeros(2, 2);
                J_lm(1,1) = dx / range_pred;
                J_lm(1,2) = dy / range_pred;
                J_lm(2,1) = -dy / q;
                J_lm(2,2) = dx / q;
                
                info = edge.info;
                
                % 累加到H和b
                idx_pose = (pose_id-1)*3+(1:3);
                idx_lm = 3*n_poses + (lm_idx-1)*2+(1:2);
                
                H(idx_pose, idx_pose) = H(idx_pose, idx_pose) + J_pose' * info * J_pose;
                H(idx_pose, idx_lm) = H(idx_pose, idx_lm) + J_pose' * info * J_lm;
                H(idx_lm, idx_pose) = H(idx_lm, idx_pose) + J_lm' * info * J_pose;
                H(idx_lm, idx_lm) = H(idx_lm, idx_lm) + J_lm' * info * J_lm;
                
                b(idx_pose) = b(idx_pose) + J_pose' * info * error;
                b(idx_lm) = b(idx_lm) + J_lm' * info * error;
                
                chi2 = chi2 + error' * info * error;
            end
        end
        
        % Levenberg-Marquardt阻尼（更保守）
        lambda = 1e-3;
        H = H + lambda * speye(size(H));
        
        % 固定第一个位姿
        for d = 1:3
            idx = (fixed_pose-1)*3 + d;
            H(idx, :) = 0;
            H(:, idx) = 0;
            H(idx, idx) = 1;
            b(idx) = 0;
        end
        
        % 求解
        dx = sparse(H) \ sparse(b);
        
        % 检查求解是否成功
        if any(isnan(dx)) || any(isinf(dx))
            fprintf('  优化失败：求解发散\n');
            x = x_prev;
            break;
        end
        
        % 发散检测（更严格）
        if iter > 3 && chi2 > chi2_prev * 1.1
            fprintf('  检测到发散，恢复上一步\n');
            x = x_prev;
            break;
        end
        
        % 步长控制（自适应）
        if norm(dx) > 10
            alpha = 0.1;  % 变化大时更保守
        elseif norm(dx) > 1
            alpha = 0.3;  % 中等变化
        else
            alpha = 0.5;  % 接近收敛时步长更大
        end
        x_prev = x;   % 保存当前状态
        x = x + alpha * dx;
        
        % 角度归一化
        for i = 1:n_poses
            x((i-1)*3+3) = wrapToPi(x((i-1)*3+3));
        end
        
        % 收敛检测
        if norm(dx) < 1e-3
            fprintf('\n  优化收敛于第%d次迭代, chi2=%.3f\n', iter, chi2);
            break;
        end
        
        chi2_prev = chi2;
    end
    
    % 提取优化后的位姿
    optimized_poses = zeros(3, n_poses);
    for i = 1:n_poses
        optimized_poses(:, i) = x((i-1)*3+1:i*3);
    end
    
    % 提取优化后的路标
    optimized_landmarks = containers.Map('KeyType', 'int32', 'ValueType', 'any');
    for i = 1:length(lm_keys)
        lm_id = lm_keys(i);
        lm_pos = x(3*n_poses + (i-1)*2+1:3*n_poses + i*2);
        optimized_landmarks(lm_id) = lm_pos;
    end
    
    % 返回结构体（关键修复）
    optimized_graph = struct();
    optimized_graph.poses = optimized_poses;
    optimized_graph.landmarks = optimized_landmarks;
    optimized_graph.landmark_map = optimized_landmarks;
    optimized_graph.edges = pose_graph.edges;
end

